library(extrafont)
loadfonts()

# $1 - Directory where the .occ files corresponding to the different experiments are stored
# $2 - Experiment duration (seconds)
args <- commandArgs(trailingOnly = TRUE)
inputFile <- args[1]
#robotNumber <- args[2]
scenario <- args[2]
rm(args)

#File location must be given with respect to the user folder
setwd("/home/deste/argos2/user/jdestefani/")

# E-Puck 1    10    11    12    13    14    15    16    17    18    19     
# Index  1:2  3:4   5:6   7:8   9:10  11:12 13:14 15:16 17:18 19:20 21:22 
# E-Puck 2     20     3      4       5       6      7       8       9
# Index  23:24 25:26  27:28  29:30   31:32   33:34  35:36   37:38   39:40
ePuckLUT <- data.frame(ncol=20,ncol=2)
colnames(ePuckLUT) <- c("x","y")
ePuckLUT[1,] <- c(1:2)
ePuckLUT[2,] <- c(23:24)
ePuckLUT[3,] <- c(27:28)
ePuckLUT[4,] <- c(29:30)
ePuckLUT[5,] <- c(31:32)
ePuckLUT[6,] <- c(33:34)
ePuckLUT[7,] <- c(35:36)
ePuckLUT[8,] <- c(37:38)
ePuckLUT[9,] <- c(39:40)
ePuckLUT[10,] <- c(3:4)
ePuckLUT[11,] <- c(5:6)
ePuckLUT[12,] <- c(7:8)
ePuckLUT[13,] <- c(9:10)
ePuckLUT[14,] <- c(11:12)
ePuckLUT[15,] <- c(13:14)
ePuckLUT[16,] <- c(15:16)
ePuckLUT[17,] <- c(17:18)
ePuckLUT[18,] <- c(19:20)
ePuckLUT[19,] <- c(21:22)
ePuckLUT[20,] <- c(25:26)


#Read data and compute errors for each cluster
inputData <- read.table(inputFile,header=TRUE)

#Open pdf file
pdf(paste(inputFile,".pdf",sep=""),family="Droid Sans")

#Define arena structure

#Scenario A
if(scenario == "A"){
  plot(range(-2,2),range(-2,2),type="n",xlab="",ylab="",main=paste(inputFile,"- Position"))
  points(1,1,cex=5)
  text(1,1,"1")
  points(-1,1,cex=5)
  text(-1,1,"2")
  points(-1,-1,cex=5)
  text(-1,-1,"3")
  points(1,-1,cex=5)
  text(1,-1,"4")
}

#Scenario B
if(scenario == "B"){
  plot(range(-2,2),range(-2,2),type="n",xlab="",ylab="",main=paste(inputFile,"- Position"))
  points(1,1,cex=5)
  text(1,1,"1")
  points(-1-0.3,1+0.3,cex=5)
  text(-1-0.3,1+0.3,"2")
  points(-1,-1,cex=5)
  text(-1,-1,"3")
  points(0,0,cex=5)
  text(0,0,"4")
}


#Scenario C
if(scenario == "C"){
  plot(range(-2,2),range(-2.55,2.55),type="n",xlab="",ylab="",paste(inputFile,"- Position"))
  points(0,2,cex=5)
  text(0,2,"1")
  points(0,1,cex=5)
  text(0,1,"2")
  points(0,0,cex=5)
  text(0,0,"3")
  points(0,-1,cex=5)
  text(0,-1,"4")
}

#Robot trajectory 
lineTypes <- c(1,2,4,5,6)
colors <- rainbow(dim(inputData)[2]/2)
robots <- seq(20)
sapply(robots,FUN=function(x) lines(x=inputData[,ePuckLUT[x,"x"]],y=inputData[,ePuckLUT[x,"y"]],col=colors[x],lty=lineTypes[x%%length(lineTypes)]))
#lines(x=inputData[,ePuckLUT[robotNumber,"x"]],y=inputData[,ePuckLUT[robotNumber,"y"]],col=colors)
par(xpd=TRUE)
legend(x="bottomright", inset=c(-0.075,0), rownames(ePuckLUT), cex=0.5, col=colors, lty=lineTypes, title="Robot")
par(xpd=FALSE)
dev.off()
